#include "map_system.h"
#include "robot.h"
#include "lidar_ai_data.h"
#include <chrono> // 用于时间测量


int main() 
{
    // 01-数据准备
    // std::string li_path = "/home/ai/wlm/gitee/map_learing/04_map_grid_cxx/CXX/data/output1.txt";
    std::string li_path = "/home/ai/wlm/gitee/map_learing/01_binocular_vision_xuce/output2.txt";



    auto res = readAndParseData(li_path);


    std::cout << "match.len: " << res.size() << std::endl;

    // std::vector<MatchedFrame> res = matchingPoseWithAI(ai_frames, lidar_frames);

    // 02-计算地图的长宽
    float min_x, max_x, min_y, max_y;
    findMinMaxXY(res, min_x, max_x, min_y, max_y);
    std::cout << "Min X: " << min_x << ", Max X: " << max_x << std::endl;
    std::cout << "Min Y: " << min_y << ", Max Y: " << max_y << std::endl;

    // 03-计算全局占用的栅格数量:
    int global_gridw, global_gridh;
    float gird_size = 0.05f;
    global_gridw = int((max_x - min_x) / gird_size);
    global_gridh = int((max_y - min_y) / gird_size);
    std::cout << "global_gridw: " << global_gridw << std::endl;
    std::cout << "global_gridh: " << global_gridh << std::endl;

    // 04-全局地图 局部地图构建
    int local_gridw = 100;
    int local_gridh = 100;

    MapSystem map_system(global_gridw, global_gridh, local_gridw, local_gridh);

    // 05-刷新地图，打印地图
    // 记录起始时间
    auto start = std::chrono::high_resolution_clock::now();
    // 输出耗时
    // int len = 300;
    int len = res.size();
    for(int i = 0 ; i < len; i++)
    {
        MatchedFrame tmp = res[i];
        std::vector<cv::Point3f> pts = tmp.xyz;
        cv::Point3f fuse_xyz = tmp.fusepose;
        cv::Point3f fuse_rpy = tmp.rpy;

        // 获取指针
        const std::vector<unsigned char>& map_ptr = map_system.getMapPointer();
        // 获取局部
        map_system.updateLocalMap(map_ptr, pts, fuse_xyz, fuse_rpy, i);
        std::vector<unsigned char> local_map = map_system.getUpdatedLocalMapData();
        // 更新全局
        map_system.updateGlobalMap(local_map);
    }


    auto end = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
    std::cout << "耗时: " << duration << " 毫秒" << std::endl;




    return 0;
}
